﻿import time
# import ctypes
from  base_mpu6050 import mpu_object
import numpy as np
# from scipy.stats import linregress
import matplotlib.pyplot as plt
import paho.mqtt.client as mqtt
import json
from scipy.stats import linregress

mpu = mpu_object()

# 卡尔曼滤波器类
class KalmanFilter:
    def __init__(self, Q=1e-5, R=0.01):
        self.Q = Q  # 过程噪声协方差
        self.R = R  # 测量噪声协方差
        self.x = 0.0  # 初始估计值
        self.P = 1.0  # 初始估计协方差
        self.K = 0.0  # 初始卡尔曼增益

    def update(self, measurement):
        # 预测步骤
        self.P = self.P + self.Q  # 更新估计协方差
        # 计算卡尔曼增益
        self.K = self.P / (self.P + self.R)
        # 更新估计值
        self.x = self.x + self.K * (measurement - self.x)
        # 更新协方差
        self.P = (1 - self.K) * self.P
        return self.x


# 初始化卡尔曼滤波器
kf = KalmanFilter()

# 存储读取的数据

# MQTT配置
broker_address = "127.0.0.1"  # 替换为实际的MQTT Broker地址
topic = "sensor/data"  # 要发布的主题

# 创建MQTT客户端并连接到Broker
client = mqtt.Client()
client.connect(broker_address, 1883, 60)

start_time = time.time()


# 模拟读取传感器数据的函数，这里用随机数生成6个浮点数
def read_sensor_data():
    a_x,a_y,a_z = mpu.read_accel()
    g_x,g_y,g_z =mpu.read_gyro()
    return a_x,a_y,a_z,g_x,g_y,g_z


def yaw_offset_correct(past_time):
    # return 0.2738*past_time + -0.8052
    return 0.28*past_time + -0.8052


# offset = -0.29
offset = 0.5-0.05
dealt_time = 0.1
current_yaw = 0
data = []

avg_raw=0
avg_filter = 0

count = 0

# 读取数据，每0.1秒读取一次，持续10分钟
while (time.time() - start_time) < 600:  # 10分钟为600秒
    sensor_data = read_sensor_data()
    current_time = time.time() - start_time
    # timestamps.append(current_time)  # 记录时间戳
    # data.append(sensor_data)  # 记录传感器数据
    raw_value = sensor_data[5]+offset

    # 使用卡尔曼滤波器平滑数据
    filtered_value = kf.update(raw_value)
    
    #积累角度偏移
    current_yaw += (raw_value*dealt_time)    
    
    #均值积累
    avg_raw += raw_value
    avg_filter+=filtered_value
    count+=1

    # 将时间戳和过滤后的数据作为消息发布到MQTT
    # payload = f"{filtered_value},{raw_value}"
    payload = {
        "filter":filtered_value,
        "raw":raw_value,
        "avg_raw":avg_raw/count,
        "avg_filter":avg_filter/count
        
    }
    # 将字典转换为JSON字符串
    payload_json = json.dumps(payload)
    # data.append([current_time,current_yaw])
    # print(payload_json)
    # 通过MQTT发布JSON数据
    client.publish(topic, payload_json)
    
    try:
   
        time.sleep(dealt_time)  # 每0.1秒
    except KeyboardInterrupt:
        # data = np.array(data)
        # for i in range(1, 2):  # 数据的第1列是时间，第2到7列是传感器数据
        #     x = data[:, 0]  # 时间列
        #     y = data[:, i]  # 传感器数据列
        #     slope, intercept, r_value, p_value, std_err = linregress(x, y)
        #     print(f"Sensor {i}: y = {slope:.4f}x + {intercept:.4f}")
        client.disconnect()

    # print(filtered_value)

# 将数据转换为NumPy数组，方便后续处理
# data = np.array(data)
# 
# # # 拟合数据列，生成y = kx + b的函数表达式
# for i in range(1, 2):  # 数据的第1列是时间，第2到7列是传感器数据
#     x = data[:, 0]  # 时间列
#     y = data[:, i]  # 传感器数据列
#     slope, intercept, r_value, p_value, std_err = linregress(x, y)
#     print(f"Sensor {i}: y = {slope:.4f}x + {intercept:.4f}")
    
# 断开MQTT连接
client.disconnect()
